#include "cartographer/rcm/imu/imu_edge.h"

namespace cartographer
{
    bool PoseUpdater::Plus(const double *x, const double *delta, double *x_plus_delta) const
    {
        Eigen::Map<const Eigen::Vector3d> _p(x);                                                    // 位移
        Eigen::Map<const Eigen::Quaterniond> _q(x + 3);                                             // 四元数
        Eigen::Map<const Eigen::Vector3d> dp(delta);                                                // 增量
        Eigen::Quaterniond dq = cartographer::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3)); // 四元数增量
        Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
        Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);
        p = _p + dp;                // 位移加法
        q = (_q * dq).normalized(); // 四元数乘法当作加法
        return true;
    }

    bool PoseUpdater::ComputeJacobian(const double *x, double *jacobian) const
    {
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
        j.topRows<6>().setIdentity();
        j.bottomRows<1>().setZero();
        return true;
    }

    /*
    PVR加法如下，可与残差算法对比看
    curr.R = NormalizeRotation(last.R * incre.R);
    curr.P = last.P + last.V * dt + 0.5f * dt * dt * GRAVITY_NEGATIVE + last.R * incre.P;
    curr.V = last.V + dt * GRAVITY_NEGATIVE + last.R * incre.V;
    */
    Eigen::Matrix<double, 15, 1> ImuOptEdge::CalculateResidual(const ImuOptNode &poseI, const ImuOptNode &poseJ, const PVR &incre) const
    {
        Eigen::Quaterniond delta_q(incre.R);
        Eigen::Matrix<double, 15, 1> residuals;
        residuals.setZero();
        // 残差P = 上时刻Q.inverse() * (当前时刻P-上时刻P) - 增量P
        residuals.block<3, 1>(OFF_P, 0) = poseI.Q.inverse() * CalcPortionP(poseI, poseJ) - incre.P;
        residuals.block<3, 1>(OFF_R, 0) = 2 * (delta_q.inverse() * (poseI.Q.inverse() * poseJ.Q)).vec();
        residuals.block<3, 1>(OFF_V, 0) = poseI.Q.inverse() * CalcPortionV(poseI, poseJ) - incre.V;
        residuals.block<3, 1>(OFF_BA, 0) = poseJ.Ba - poseI.Ba;
        residuals.block<3, 1>(OFF_BG, 0) = poseJ.Bg - poseI.Bg;
        return residuals;
    }
} // namespace cartographer
